#include "stdafx.h"
#include "cvxKFCalib.h"
#include <time.h>
#include "cvxCapture.h"
#include "cvxSf.h"
#include "CCamera.h"
#include "cvxAnn.h"
#include <utility>
#include "cvxGeometry.h"

using std::pair;
using std::make_pair;

CvxKFCalib::CvxKFCalib()
{
	m_pCriteria = NULL;
	m_pCamera = NULL;
	m_pkdTree = NULL;
}

CvxKFCalib::~CvxKFCalib()
{
	if (m_pCamera)
	{
		delete m_pCamera, m_pCamera = NULL;
	}
	if (m_pkdTree)
	{
		delete m_pkdTree, m_pkdTree = NULL;
	}
}

bool CvxKFCalib::GetNoblurFrames(const string &vf, const string &ptf, CvxBoard *ptarget, CvSize &imageSize)
{
	assert(m_pCriteria);
	//get clear image corners;
	vector<vector<Point2f>> ptsVec;
	vector<int>   idxVec;
	vector<float> offsetVec;  //measure blur

	CvSize patternSize = ptarget->GetPatternSize();
	bool isPtsOk = false;	
	//get corner position
	if (!(m_pCriteria->_readPtsFromFile))
	{		
		//get pts from video file
		double tt = clock();
		string savef = m_pCriteria->_saveFolder + string("video_pts.txt");
		isPtsOk = CvxKFCalib::SaveCorners(vf, savef, ptarget, m_pCriteria->_isTracking, imageSize);
		if (m_pCriteria->_printOutMsg) printf("get corner from video cost time = %f", clock() - tt);

		//load pts from save file
		if (isPtsOk)
		{
			isPtsOk = CvxKFCalib::LoadCorners(savef, idxVec, ptsVec, offsetVec, patternSize, imageSize);
		}
	}
	else
	{
		isPtsOk = CvxKFCalib::LoadCorners(ptf, idxVec, ptsVec, offsetVec, patternSize, imageSize);		
	}
	if (!isPtsOk)
	{
		fprintf(stderr, "can not get corner position\n");
		return false;
	}

	if (m_pCriteria->_printOutMsg && m_pCriteria->_pCap)
	{
		int number = (int)(cvGetCaptureProperty(m_pCriteria->_pCap, CV_CAP_PROP_FRAME_COUNT));
		printf("video has %d frames \n", number);
	}

	if (m_pCriteria->_printOutMsg) printf("%d frames has corners.\n", idxVec.size());

	//get clear marker frames;
	for (int i = 0; i<offsetVec.size(); ++i)
	{
		if (offsetVec[i] < m_pCriteria->_clearThd)
		{
			m_pts.push_back(ptsVec[i]);
			m_idxes.push_back(idxVec[i]);
			m_offsets.push_back(offsetVec[i]);
		}
	}
	if (m_pCriteria->_printOutMsg) printf("%d frames has blur-free markers.\n", m_idxes.size());
	if (m_pCriteria->_writeFile) 
		CvxKFCalib::SaveIntermResult(vf, m_pCriteria->_saveFolder+ string("clear_corner.txt"), 
							m_idxes, m_pts, m_offsets, patternSize, imageSize);
	if (m_pCriteria->_checkCornerPos && m_pCriteria->_pCap)
	{
// 		CvxArbitraryCapture arbCap(m_pCriteria->_pCap);
// 		IplImage *frame = arbCap.GetFrame(0);
// 		IplImage *copyFrame = cvCloneImage(frame);
// 		IplImage *showImage = cvCreateImage(cvSize(frame->width/2, frame->height/2), IPL_DEPTH_8U, 3);
// 		for (int i = 0; i<m_idxes.size(); ++i)
// 		{
// 			frame = arbCap.GetFrame(m_idxes[i]);
// 			cvCopy(frame, copyFrame);
// 			cv::drawChessboardCorners(Mat(copyFrame), cvSize(16, 12), m_pts[i], true);
// 			cvResize(copyFrame, showImage);
// 			cvShowImage("corners", showImage);
// 			cvWaitKey(30);
// 		}
// 		cvReleaseImage(&copyFrame);
// 		cvReleaseImage(&showImage);
	}
	return m_idxes.size() > 100;
}



void CvxKFCalib::GetInitialFrameIndex(vector<int> &idxVec)
{
	/************************************************************************/
	/* 
	the algorithm first divides the video to sub video
	when the chessboard points have large displace between consequent frames.
	then it randomly selects one frame from each sub video
	*/
	/************************************************************************/
	idxVec.clear();
	//get initial key frames
	int sIdx = 0;        //start index of sub sequence
	int eIdx = 0;        //end index of sub sequence
	for (int i = 1; i<m_idxes.size(); ++i)
	{	
		float dif = 0.0f;
		for (int j = 0; j<m_pts[i].size(); ++j)
		{
			Point2f p1 = m_pts[i][j];
			Point2f p2 = m_pts[i-1][j];
			dif += sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y)+0.000001);
		}
		dif /= m_pts[i].size();

		if (dif > m_pCriteria->_difPoseThd )
		{
			eIdx = i;
			assert(eIdx > sIdx);				
			//random index from sIdx to eIdx
			int rIdx = sIdx + rand()%(eIdx - sIdx);
			idxVec.push_back(rIdx);
			sIdx = eIdx;
		}
	}

	if (m_pCriteria->_printOutMsg) printf("different pose number is %d.\n", idxVec.size());
	if (m_pCriteria->_printOutMsg)
	{
		printf("initial index numbers: ");
		for (int i = 0; i<idxVec.size(); ++i)
		{
			printf("%d ", m_idxes[idxVec[i]]);
		}
		printf("\n");
	}
	if (m_pCriteria->_writeFile && m_pCriteria->_pCap)
	{
		CvxArbitraryCapture arbCap(m_pCriteria->_pCap);
		vector<IplImage*> imageVec;
		for (int i = 0; i<idxVec.size(); ++i)
		{
			IplImage *frame = arbCap.GetFrame(m_idxes[idxVec[i]]);
			if (frame)
			{
				IplImage *copyFrame = cvCloneImage(frame);				
				imageVec.push_back(copyFrame);
				if (m_pCriteria->_showFrame)
				{
					IplImage *smallImage = cvCreateImage(cvSize(copyFrame->width/2, copyFrame->height/2), IPL_DEPTH_8U, 3);
					cvResize(copyFrame, smallImage);
					cvShowImage("initial frames", smallImage);
					cvWaitKey(200);
					cvReleaseImage(&smallImage);
				}
			}
		}
		if (m_pCriteria->_showFrame)
		{
			cvDestroyWindow("initial frames");
		}
		if (m_pCriteria->_printOutMsg) printf("save %d images as initial selection.\n", imageVec.size());
	//	CvxSF::saveImageVec(imageVec, string("initial_select_"), string(".png"), m_pCriteria->_saveFolder.c_str());
		CvxSF::releaseImage(imageVec);			
	}
}

void CvxKFCalib::initCamera(const vector<int> &idxVec, const CvSize &imageSize, const CvxBoard *ptarget)
{
	assert(idxVec.size() > 5);

	//get camera from key frames
	m_pCamera = new CCamera;
	assert(m_pCamera);
	
	//target image position 
	vector<vector<Point2f>> pts2dvec(idxVec.size());
	for (int i = 0; i<idxVec.size(); ++i)
	{
		pts2dvec[i] = m_pts[idxVec[i]];
	}
	//target 3d geometry
	vector<Point3f> pts3d;
	ptarget->Get3Dpts(pts3d);

	m_pCamera->setFeature2d(pts2dvec);
	m_pCamera->setCorners3d(pts3d);
	m_pCamera->setImageSize(imageSize);
	m_pCamera->calib(m_pCriteria->_cameraFlags, true);
	if (m_pCriteria->_writeFile)
	{
		m_pCamera->saveIntrinsic(m_pCriteria->_saveFolder + string("initial_intrinsic.txt"));
		printf("save file: %s\n", string("initial_intrinsic.txt").c_str());
	}

	if (m_pCriteria->_printOutMsg) printf("get initial camera.\n");		
}

void CvxKFCalib::updateCamera(const CvSize &imageSize, const CvxBoard *ptarget)
{
	assert(m_optimalIdxes.size() > 5);
	assert(m_pCamera);

	//get camera from key frames
	delete m_pCamera, m_pCamera = NULL;
	m_pCamera = new CCamera;
	assert(m_pCamera);

	//target image position 
	vector<vector<Point2f>> pts2dvec(m_optimalIdxes.size());
	for (int i = 0; i<m_optimalIdxes.size(); ++i)
	{
		pts2dvec[i] = m_pts[m_optimalIdxes[i]];
	}
	//target 3d geometry
	vector<Point3f> pts3d;
	ptarget->Get3Dpts(pts3d);

	m_pCamera->setFeature2d(pts2dvec);
	m_pCamera->setCorners3d(pts3d);
	m_pCamera->setImageSize(imageSize);
	m_pCamera->calib(m_pCriteria->_cameraFlags, true);
	if (m_pCriteria->_writeFile)
	{
		m_pCamera->saveIntrinsic(m_pCriteria->_saveFolder + string("update_camera_intrinsic.txt"));
		printf("save file: %s\n", string("update_camera_intrinsic.txt").c_str());
	}

	if (m_pCriteria->_printOutMsg) printf("update camera complete.\n");		
}

void CvxKFCalib::initKdTree(const vector<int> &idxVec, const CvxBoard *ptarget)
{
	assert(m_pCamera);
	assert(ptarget);
	assert(idxVec.size() > 5);

	m_pkdTree= new CvxAnn(3, 2);
	assert(m_pkdTree);
	//get kd-tree from selected index frame points position
	double *pdata = new double[3 * idxVec.size()];
	assert(pdata);
	memset(pdata, 0, sizeof(pdata[0]) * 3 * idxVec.size());

	Mat intrinsicM  = m_pCamera->getIntrinsicMatrix();
	Mat distortionM = m_pCamera->getDistortionCoeffs();
	vector<Point3f> pts3d = (m_pCamera->getFeaturePoints3d())[0];

	Mat rvec;
	Mat tvec;
	Vec3d normal;
	Point3f pos;
	for (int i = 0; i<idxVec.size(); ++i)
	{		
		//get normal of each target plane
		cv::solvePnP(pts3d, m_pts[idxVec[i]], intrinsicM, distortionM, rvec, tvec, false);
		CvxGeometry::GetPlaneNormalAndPatternCenterPosition(rvec, tvec, ptarget->GetUnitLength(), 
											ptarget->GetPatternSize(), normal, pos);
		pdata[3*i+0] = normal[0];
		pdata[3*i+1] = normal[1];
		pdata[3*i+2] = normal[2];
	}
	m_pkdTree->setTree(pdata, idxVec.size());
	if (m_pCriteria->_printOutMsg) printf("build kd-tree.\n");
	if (m_pCriteria->_writeFile)
	{
		string savename = m_pCriteria->_saveFolder + string("initial_kd_tree.txt");
		FILE *pf = fopen(savename.c_str(), "w");
		assert(pf);
		for (int i = 0; i<idxVec.size(); ++i)
		{
			fprintf(pf, "%f %f %f\n",pdata[3*i], pdata[3*i+1], pdata[3*i+2]);
		}
		fclose(pf);		
		printf("save file: %s\n", savename.c_str());
	}
	if (pdata)
	{
		delete pdata, pdata = NULL;
	}
}



void CvxKFCalib::updateKdTree(const vector<int> &idxVec, const CvxBoard *ptarget)
{
	assert(m_pkdTree);
	assert(m_pCamera);
	assert(ptarget);
	assert(idxVec.size() > 5);

	delete m_pkdTree, m_pkdTree = NULL;

	m_pkdTree= new CvxAnn(3, 2);
	assert(m_pkdTree);
	//get kd-tree from selected index frame points position
	double *pdata = new double[3 * idxVec.size()];
	assert(pdata);
	memset(pdata, 0, sizeof(pdata[0]) * 3 * idxVec.size());

	Mat intrinsicM  = m_pCamera->getIntrinsicMatrix();
	Mat distortionM = m_pCamera->getDistortionCoeffs();
	vector<Point3f> pts3d = (m_pCamera->getFeaturePoints3d())[0];

	Mat rvec;
	Mat tvec;
	Vec3d normal;
	Point3f pos;
	for (int i = 0; i<idxVec.size(); ++i)
	{		
		//get normal of each target plane
		cv::solvePnP(pts3d, m_pts[idxVec[i]], intrinsicM, distortionM, rvec, tvec, false);
		CvxGeometry::GetPlaneNormalAndPatternCenterPosition(rvec, tvec, ptarget->GetUnitLength(), 
					ptarget->GetPatternSize(), normal, pos);
		pdata[3*i+0] = normal[0];
		pdata[3*i+1] = normal[1];
		pdata[3*i+2] = normal[2];
	}
	m_pkdTree->setTree(pdata, idxVec.size());
	if (m_pCriteria->_printOutMsg) printf("update kd-tree.\n");
	if (m_pCriteria->_writeFile)
	{
		string savename = m_pCriteria->_saveFolder + string("update_kd_tree.txt");
		FILE *pf = fopen(savename.c_str(), "w");
		assert(pf);
		for (int i = 0; i<idxVec.size(); ++i)
		{
			fprintf(pf, "%f %f %f\n",pdata[3*i], pdata[3*i+1], pdata[3*i+2]);
		}
		fclose(pf);		
		printf("save file: %s\n", savename.c_str());
	}
	if (pdata)
	{
		delete pdata, pdata = NULL;
	}

}

bool CvxKFCalib::AddFrames(const CvxBoard *ptarget, const vector<int> &curIdxVec)
{
	assert(m_pkdTree);
	assert(m_pCamera);

	m_optimalIdxes = curIdxVec;

	Mat intrinsicM  = m_pCamera->getIntrinsicMatrix();
	Mat distortionM = m_pCamera->getDistortionCoeffs();
	vector<Point3f> pts3d = (m_pCamera->getFeaturePoints3d())[0];

	Mat rvec;
	Mat tvec;
	Vec3d normal;
	Point3f pos;
	bool isUpdate = false;
	int addNum = 0;
	vector<Vec3d> normVec(m_pts.size());  //for visualization
	for (int i = 0; i<m_pts.size(); ++i)
	{		
		//get normal of each target plane
		double data[3];
		double dis[2];
		int idxes[2];
		cv::solvePnP(pts3d, m_pts[i], intrinsicM, distortionM, rvec, tvec, false);
		CvxGeometry::GetPlaneNormalAndPatternCenterPosition(rvec, tvec, ptarget->GetUnitLength(), 
										                    ptarget->GetPatternSize(), normal, pos);
		data[0] = normal[0];
		data[1] = normal[1];
		data[2] = normal[2];
		if (m_pCriteria->_writeFile)
		{
			normVec[i] = normal;
		}

		m_pkdTree->query(data, dis, idxes);
		//minimal distance to kd-tree less than threshold
		if (dis[0] > m_pCriteria->_kdDisThd)
		{
		//	printf("dis = %f\n", dis[0]);
			//update kd-tree immediately
			m_optimalIdxes.push_back(i);
			updateKdTree(m_optimalIdxes, ptarget);
			isUpdate = true;			
			addNum++;
		}
	}

	if (isUpdate && m_pCriteria->_writeFile && m_pCriteria->_pCap)
	{		
		CvxArbitraryCapture arbCap(m_pCriteria->_pCap);		
		for (int i = curIdxVec.size(); i<m_optimalIdxes.size(); ++i)
		{
			IplImage *frame = arbCap.GetFrame(m_idxes[m_optimalIdxes[i]]);
			char buf[256] = {NULL};
			sprintf(buf, "extra_%d.png", i);
			cvSaveImage((m_pCriteria->_saveFolder + string(buf)).c_str(), frame);
			if (m_pCriteria->_showFrame)
			{
				IplImage *smallImage = cvCreateImage(cvSize(frame->width/2, frame->height/2), IPL_DEPTH_8U, 3);
				cvResize(frame, smallImage);
				cvShowImage("new added frames", smallImage);
				cvWaitKey(500);
				cvReleaseImage(&smallImage);
			}
		}
		cvDestroyWindow("new added frames");
		printf("save extra images\n");
	}
	if (m_pCriteria->_printOutMsg)
	{
		printf("add %d frames.\n", addNum);
	}
	if (m_pCriteria->_writeFile)
	{
		string savename = m_pCriteria->_saveFolder + string("plane_normal.txt");
		FILE *pf = fopen(savename.c_str(), "w");
		assert(pf);
		for (int i = 0; i<normVec.size(); ++i)
		{
			fprintf(pf, "%f %f %f\n",normVec[i][0], normVec[i][1], normVec[i][2]);
		}
		fclose(pf);		
		printf("save file: %s\n", savename.c_str());

	}
	return isUpdate;
}

bool CvxKFCalib::DeleteFrames(const CvxBoard *ptarget, const vector<int> &curIdxVec)
{
	assert(m_pkdTree);
	assert(m_pCamera);

	m_optimalIdxes = curIdxVec;

	Mat intrinsicM  = m_pCamera->getIntrinsicMatrix();
	Mat distortionM = m_pCamera->getDistortionCoeffs();
	vector<Point3f> pts3d = (m_pCamera->getFeaturePoints3d())[0];

	Mat rvec;
	Mat tvec;
	Vec3d normal;
	Point3f pos;
	vector<int> deleteIdx;  //delete index numbers
	for (int i = 0; i<m_optimalIdxes.size(); ++i)
	{		
		//get normal of each plane in the current kd-tree
		double data[3];
		double dis[2];
		int idxes[2];
		cv::solvePnP(pts3d, m_pts[m_optimalIdxes[i]], intrinsicM, distortionM, rvec, tvec, false);
		CvxGeometry::GetPlaneNormalAndPatternCenterPosition(rvec, tvec, ptarget->GetUnitLength(), 
															ptarget->GetPatternSize(), normal, pos);
		data[0] = normal[0];
		data[1] = normal[1];
		data[2] = normal[2];

		m_pkdTree->query(data, dis, idxes);
		//minimal distance to kd-tree less than threshold
		if (dis[1] < m_pCriteria->_kdDisThd)
		{
			int dIdx = m_optimalIdxes[i];
			if (m_offsets[m_optimalIdxes[i]]< m_offsets[m_optimalIdxes[idxes[1]]])
			{
				dIdx = m_optimalIdxes[idxes[1]];
			}
			if (std::find(deleteIdx.begin(), deleteIdx.end(), dIdx) == deleteIdx.end())
			{
				deleteIdx.push_back(dIdx);
			}			
		}
	}
	
	if (deleteIdx.size() > 0)
	{
		//delete frames from current optimal indexes
	//	std::unique(deleteIdx.begin(), deleteIdx.end());
		for (int i = 0; i<deleteIdx.size(); ++i)
		{
			printf("%d\n", deleteIdx[i]);
		}
		if (m_pCriteria->_printOutMsg) printf("delete %d frames\n", deleteIdx.size());
		for (int i = 0; i<deleteIdx.size(); ++i)
		{
			std::remove(m_optimalIdxes.begin(), m_optimalIdxes.end(), deleteIdx[i]);
		}
		m_optimalIdxes.resize(m_optimalIdxes.size() - deleteIdx.size());

		if (m_pCriteria->_writeFile && m_pCriteria->_pCap)
		{		
			CvxArbitraryCapture arbCap(m_pCriteria->_pCap);		
			for (int i = 0; i<deleteIdx.size(); ++i)
			{
				IplImage *frame = arbCap.GetFrame(m_idxes[deleteIdx[i]]);
				char buf[256] = {NULL};
				sprintf(buf, "delete_%d.png", i);
				cvSaveImage((m_pCriteria->_saveFolder + string(buf)).c_str(), frame);
				if (m_pCriteria->_showFrame)
				{
					IplImage *smallImage = cvCreateImage(cvSize(frame->width/2, frame->height/2), IPL_DEPTH_8U, 3);
					cvResize(frame, smallImage);
					cvShowImage("new discarded frames", smallImage);
					cvWaitKey(500);
					cvReleaseImage(&smallImage);
				}
			}
			cvDestroyWindow("new discarded frames");
			printf("save delete images\n");
		}
		return true;
	}
	return false;
}

bool CvxKFCalib::select(const string &vf, const string &ptf, 
						CvxBoard *ptarget, const KFCalibSelectCriteria &criteria,  vector<int> &kFI)
{
	//check parameters
	assert(ptarget);

	kFI.clear();
	m_pts.clear(); 
	m_idxes.clear();          
	m_offsets.clear();
	CvSize patternSize = ptarget->GetPatternSize();
	CvSize imageSize;
	float  uL = ptarget->GetUnitLength();   //unit length of target

	//get no blur frames and points positions
	m_pCriteria = const_cast<KFCalibSelectCriteria*>(&criteria);
	if (!GetNoblurFrames(vf, ptf, ptarget, imageSize))
	{
		return false;
	}

	//get sub division from video	
	vector<int> initialIdxVec;  //index of clear pts,
	GetInitialFrameIndex(initialIdxVec);	

	//get initial camera from initial frames
	initCamera(initialIdxVec, imageSize, ptarget);
	
	//get initial kdtree from initial index
	initKdTree(initialIdxVec, ptarget);

	bool isAdd = true;
	bool isDelete = true;
	int iteriNumb = 0;

	
	//refine key frames iteratively
	while ((isAdd || isDelete) && iteriNumb < 4)
	{
		if (m_pCriteria->_printOutMsg)
		{
			printf("\n iterate = %d\n", iteriNumb);
			iteriNumb++;
		}
		//add frame to kd-tree, delete frame from kd-tree
		isAdd = AddFrames(ptarget, initialIdxVec);	
		isDelete = DeleteFrames(ptarget, m_optimalIdxes);
		//update kd-tree and camera
		updateKdTree(m_optimalIdxes, ptarget);
		initialIdxVec = m_optimalIdxes;
		//update camera
		updateCamera(imageSize, ptarget);	
	
		if (m_pCriteria->_printOutMsg)
		{
			printf("optimized index number = %d\n", m_optimalIdxes.size());
		}
	}

	//save final index and images

	if (m_pCriteria->_writeFile && m_pCriteria->_pCap)
	{		
		CvxArbitraryCapture arbCap(m_pCriteria->_pCap);		
		for (int i = 0; i<m_optimalIdxes.size(); ++i)
		{
			IplImage *frame = arbCap.GetFrame(m_idxes[m_optimalIdxes[i]]);
			char buf[256] = {NULL};
			sprintf(buf, "final_%d.png", i);
			cvSaveImage((m_pCriteria->_saveFolder + string(buf)).c_str(), frame);

			if (m_pCriteria->_showFrame)
			{
				IplImage *smallImage = cvCreateImage(cvSize(frame->width/2, frame->height/2), IPL_DEPTH_8U, 3);
				cvResize(frame, smallImage);
				cvShowImage("final frames", smallImage);
				cvWaitKey(200);
				cvReleaseImage(&smallImage);
			}
		}
		printf("save final images\n");
		FILE *pf = fopen((m_pCriteria->_saveFolder + string("final_idx.txt")).c_str(), "w");
		assert(pf);
		fprintf(pf, "%d # number of frames\n", m_optimalIdxes.size());
		printf("%d # number of frames\n",  m_optimalIdxes.size());
		for (int i = 0; i<m_optimalIdxes.size(); ++i)
		{
			fprintf(pf, "%d ", m_idxes[m_optimalIdxes[i]]);
			printf("%d ", m_idxes[m_optimalIdxes[i]]);
		}
		fclose(pf);
	}
	return true;
}

bool CvxKFCalib::SaveCorners(const string &vf, const string &savef, const CvxBoard *ptarget, 
							 bool isTracking, CvSize &imageSize)
{
	assert(ptarget);

	int pw = ptarget->GetPatternSize().width;
	int ph = ptarget->GetPatternSize().height;
	vector<Point2f> prePts;
	vector<Point2f> curPts;
	vector<Point2f> nextPts;
	bool isPreOk  = false;
	bool isCurOk  = false;
	bool isNextOk = false;

	vector<CvxMarker> preMarkers;
	vector<CvxMarker> curMarker;

	CvCapture *cp = cvCaptureFromFile(vf.c_str());
	if (!cp)
	{
		fprintf(stderr, "can not open file %s\n", vf.c_str());
		return false;
	}
	int count = cvGetCaptureProperty(cp, CV_CAP_PROP_FRAME_COUNT);
	if (count < 5)
	{		
		fprintf(stderr, "frame number %d is too small%s\n", count);
		return false;
	}
	
	FILE *pf = fopen(savef.c_str(), "w");
	if (!pf)
	{
		fprintf(stderr, "can not create file %s\n", savef.c_str());
		return false;
	}
	fprintf(pf, "%s # video name\n", vf.c_str());
	fprintf(pf, "%d %d # pattern size\n", pw, ph);	

	//0
	IplImage *frame = cvQueryFrame(cp);
	assert(frame);
	isPreOk = ptarget->FindCorners(frame, false, preMarkers, curMarker, prePts);
	imageSize = cvSize(frame->width, frame->height);
	// 1
	frame = cvQueryFrame(cp);
	assert(frame);
	isCurOk = ptarget->FindCorners(frame, false, preMarkers, curMarker, curPts);
	// 2
	frame = cvQueryFrame(cp);
	assert(frame);
	isNextOk = ptarget->FindCorners(frame, false, preMarkers, curMarker, nextPts);

	fprintf(pf, "%d %d # frame size\n", frame->width, frame->height);	

	int curIdx = 1;
	int valid_num = 0;
	int test_num = 300;
#define SAVECORNERS_TEST 0
#if SAVECORNERS_TEST
	IplImage *copyFrame = cvCloneImage(frame);
	IplImage *showFrame = cvCreateImage(cvSize(frame->width/2, frame->height/2), IPL_DEPTH_8U, 3);
#endif
	while (frame)
	{
		if (isPreOk && isCurOk && isNextOk)
		{
			valid_num++;
			assert(prePts.size() == curPts.size() && curPts.size() == nextPts.size());
			float offset_avg = 0.0;
			for (int j = 0; j<prePts.size(); ++j)
			{
				Point2f p1 = prePts[j];
				Point2f p2 = curPts[j];
				Point2f p3 = nextPts[j];
				offset_avg += (p2.x - p1.x) * (p2.x - p1.x) +  (p2.y - p1.y) * (p2.y - p1.y);
				offset_avg += (p2.x - p3.x) * (p2.x - p3.x) +  (p2.y - p3.y) * (p2.y - p3.y);
			}
			offset_avg /= 2.0f * (pw * ph);
			fprintf(pf, "%d # frame index in video\n", curIdx);			
			fprintf(pf, "%f # average offset between previous and next frames\n", offset_avg);
			for (int j = 0; j<curPts.size(); ++j)
			{
				fprintf(pf, "%f %f ", curPts[j].x, curPts[j].y);
			}
			fprintf(pf, "\n");

#if SAVECORNERS_TEST
			//test
			cvCopy(frame, copyFrame);
			ptarget->DrawCorners(copyFrame, curPts);
			cvResize(copyFrame, showFrame);
			cvShowImage("corners", showFrame);
			cvWaitKey(30);
#endif
			
		}
		curIdx++;
		//get next frame
		frame = cvQueryFrame(cp);		
		if (frame)
		{
			prePts.clear();
			isPreOk = isCurOk;
			prePts = curPts;

			curPts.clear();
			isCurOk = isNextOk;
			curPts = nextPts;

			nextPts.clear();
			isNextOk = ptarget->FindCorners(frame, isTracking, preMarkers, curMarker, nextPts);
			if (isTracking && isNextOk)
			{				
				preMarkers = curMarker;				
			}
			else
			{
				preMarkers.clear();
			}
			curMarker.clear();
		}
	}
	fprintf(pf, "%d # end if file\n", -1);

	cvReleaseCapture(&cp);
	fclose(pf);
	return true;
}

bool CvxKFCalib::LoadCorners(const string &fileName, vector<int> &idxVec, 
							 vector<vector<Point2f>> &ptsVec, vector<float> &offsetVec,
							 CvSize &patternSize, CvSize &frameSize)
{
	assert(idxVec.empty());
	assert(ptsVec.empty());
	assert(offsetVec.empty());

	/************************************************************************/
	/* 
	test_image_video\key_frame_selection\top_100_1.avi # video name
	15 9 # pattern size
	1920 1080 # frame size
	156 # frame index in video
	814.903809 # average offset between previous and next frames
	*/
	/************************************************************************/

	FILE *pf = fopen(fileName.c_str(), "r");
	if (!pf)
	{
		fprintf(stderr, "can not open file %s\n", fileName.c_str());
		return false;
	}
	char line[256] = {NULL};
	fgets(line, 256, pf);
	fgets(line, 256, pf);
	sscanf(line, "%d %d", &(patternSize.width), &(patternSize.height));			
	fgets(line, 256, pf);
	sscanf(line, "%d %d", &(frameSize.width), &(frameSize.height));
	memset(line, 0, sizeof(line));

	int idx;
	float avg_offset;
	vector<Point2f> onePts(patternSize.width * patternSize.height);
	while(fgets(line, 256, pf))
	{
		sscanf(line, "%d", &idx);
		if (idx == -1)//end of file
		{
			break;
		}
		fgets(line, 256, pf);
		sscanf(line, "%f", &avg_offset);

		for (int i = 0; i<patternSize.width * patternSize.height; ++i)
		{
			fscanf(pf, "%f %f ", &(onePts[i].x), &(onePts[i].y));
		}
		idxVec.push_back(idx);
		offsetVec.push_back(avg_offset);
		ptsVec.push_back(onePts);
	}
	fclose(pf);
	return true;
}

bool CvxKFCalib::SaveIntermResult(const string &vf, const string &savef, vector<int> &idxVec, 
								  vector<vector<Point2f>> &ptsVec, vector<float> &offsetVec,
								  const CvSize &patternSize, const CvSize &frameSize)
{
	assert(idxVec.size() == ptsVec.size() && ptsVec.size() == offsetVec.size());

	FILE *pf = fopen(savef.c_str(), "w");
	if (!pf)
	{
		fprintf(stderr, "can not open %s.\n", savef.c_str());
		return false;
	}
	fprintf(pf, "%s # video file.\n", vf.c_str());
	fprintf(pf, "%d %d # pattern size.\n", patternSize.width, patternSize.height);
	fprintf(pf, "%d %d # frame size.\n", frameSize.width, frameSize.height);
	fprintf(pf, "%d # frame number.\n", idxVec.size());

	for (int i = 0; i<idxVec.size(); ++i)
	{
		fprintf(pf, "%d %f # index offset\n", idxVec[i], offsetVec[i]);
		for (int j = 0; j<ptsVec[i].size(); ++j)
		{
			fprintf(pf, "%f %f ", ptsVec[i][j].x, ptsVec[i][j].y);
		}
		fprintf(pf, "\n");
	}
	fclose(pf);
}

bool CvxKFCalib::SavePose(const string &savef, const vector<Point3f> &poseVec)
{
	FILE *pf = fopen(savef.c_str(), "w");
	if (!pf)
	{
		fprintf(stderr, "can not open %s\n", savef.c_str());
		return false;
	}
	fprintf(pf, "%d # pose numbers. 180 - theta, phi, z-distance\n", poseVec.size());
	for (int i = 0; i<poseVec.size(); ++i)
	{
		fprintf(pf, "%f %f %f\n", poseVec[i].x, poseVec[i].y, poseVec[i].z);
	}
	fclose(pf);
}

bool CvxKFCalib::SaveIdx(const string &savef, const vector<vector<int>> &idxVec)
{
	FILE *pf = fopen(savef.c_str(), "w");
	if (!pf)
	{
		fprintf(stderr, "can not open %s\n", savef.c_str());
		return false;
	}
	fprintf(pf, "# candidate index\n");
	for (int i = 0; i<idxVec.size(); ++i)
	{
		fprintf(pf, "%d\n", i);
		for (int j = 0; j<idxVec[i].size(); ++j)
		{
			fprintf(pf, "%d ", idxVec[i][j]);
		}
		fprintf(pf, "\n\n");
	}
	fclose(pf);
}

float CvxKFCalib::angleDifference(float angleA, float angleB)
{
	if (angleA < angleB)
	{
		std::swap(angleA, angleB);
	}
	float dif = angleA - angleB;
	if (dif > 180.0)
	{
		dif = angleB + 360 - angleA;
	}
	return dif;
}



